#!/usr/bin/python3
import serial
from serial import SerialException
import time
import struct
import csv
import numpy as np
import threading
import psutil
import os
import sys
import wiringpi

# 串口配置 Serial Port Configuration
class SerialConfig:
    # 串口号
    portName = ''
    # 波特率
    baud = 115200

# 设备实例 Device instance
class DeviceModel:
    # region 属性 attribute

    # 设备名称 deviceName
    deviceName = "DT03d5 Depth Sensor"

    # 设备modbus ID,默认为0x01
    ADDR = 0x01

    # 设备数据字典 Device Data Dictionary
    deviceData = {}

    # 设备是否开启
    isOpen = False

    # 是否循环读取 Whether to loop read
    loop = False

    # 串口 Serial port
    serialPort = None

    # 串口配置 Serial Port Configuration
    serialConfig = SerialConfig()

    # 临时数组 Temporary array
    TempBytes = []

    # 起始寄存器 Start register
    statReg = None

    # 当第一次检查设备ID正确，说明设备真的初始化成功了
    Init = False
    # endregion

    def checkSum(self, data):
        CS = 0x000000
        for d in data:
            CS = CS + ((d - 0x30) & 0xFF)
        return CS

    def __init__(self, deviceName, portName, baud, ADDR, callback_method, idCheckCallback):
        print("{} : 初始化设备模型".format(self.deviceName))
        # 设备名称（自定义） Device Name
        self.deviceName = deviceName
        # 串口号 Serial port number
        self.serialConfig.portName = portName
        # 串口波特率 baud
        self.serialConfig.baud = baud
        # modbus ID 设备地址
        self.ADDR = ADDR
        self.deviceData = {}
        self.callback_method = callback_method
        self.idCheckCallback = idCheckCallback

    # region 获取设备数据 Obtain device data

    # 设置设备数据 Set device data
    def set(self, key, value):
        # 将设备数据存到键值 Saving device data to key values
        self.deviceData[key] = value

    # 获得设备数据 Obtain device data
    def get(self, key):
        # 从键值中获取数据，没有则返回None Obtaining data from key values
        if key in self.deviceData:
            return self.deviceData[key]
        else:
            return None

    # 删除设备数据 Delete device data
    def remove(self, key):
        # 删除设备键值
        del self.deviceData[key]

    # endregion

    # 打开设备 open Device
    def openDevice(self):
        # 先关闭端口 Turn off the device first
        print("{} : 检查端口: {}".format(self.deviceName ,self.serialConfig.portName))
        self.closeDevice()
        try:
            self.serialPort = serial.Serial(self.serialConfig.portName, self.serialConfig.baud, timeout=0.5)
            self.isOpen = True
            print("{} : {}已打开".format(self.deviceName ,self.serialConfig.portName))
            # 开启一个线程持续监听串口数据 Start a thread to continuously listen to serial port data
            t = threading.Thread(target=self.readDataTh, args=("Depth Data-Received-Thread", 10,))
            t.start()
            print("{} : 数据接收线程开启成功".format(self.deviceName))
            return True
        except SerialException:
            print("{} : 打开 ".format(self.deviceName) + self.serialConfig.portName + " 失败")
            return False
 
    # 监听串口数据线程 Listening to serial data threads
    def readDataTh(self, threadName, delay):
        print("{} : 启动 {} 线程".format(self.deviceName, threadName))
        while True:
            # 如果串口打开了
            if self.isOpen:
                try:
                    tLen = self.serialPort.inWaiting()
                    if tLen > 0:
                        data = self.serialPort.read(tLen)
                        self.onDataReceived(data)
                except Exception as ex:
                    pass
                    # print("{} : ERROR {}".format(self.deviceName ,ex))
            else:
                time.sleep(0.1)
                self.idCheckCallback(self,0x00, 0x02) # 设备串口丢失
                print("{} : ERROR: {} 线程中,检测到串口 {} 未打开".format(self.deviceName , threadName, self.serialConfig.portName))
                sys.exit(1)
                break

    # 关闭设备  close Device
    def closeDevice(self):
        if self.serialPort is not None:
            self.serialPort.close()
            print("{} : {} 端口关闭".format(self.deviceName , self.serialConfig.portName))
        else:
            print("{} : {} 端口已关闭".format(self.deviceName, self.serialConfig.portName))
        self.isOpen = False

    # region 数据解析 data analysis

    # 串口数据处理  Serial port data processing
    def onDataReceived(self, data):
        tempdata = bytes.fromhex(data.hex())
        for val in tempdata:
            self.TempBytes.append(val)
            # 判断数据头是否正确
            if self.TempBytes[0] != 0x73:
                del self.TempBytes[0]
                continue
            # 判断功能字是否为0x44
            if len(self.TempBytes) > 2:
                if self.TempBytes[1] == 0x44: # 读取传感器数据
                    if len(self.TempBytes) < 4:
                        continue
                    else:
                        ID_get = (self.TempBytes[2] - 0x30) * 10 + (self.TempBytes[3] - 0x30)
                        if (ID_get != self.ADDR):
                            del self.TempBytes[0]
                            continue
                        else:   # 设备ID正确
                            # 检查数据量
                            if len(self.TempBytes) < 22:
                                continue
                            else: # 数据量够了
                                # CS校验
                                CS = self.checkSum(self.TempBytes[2:19])
                                CS_str = str(CS).zfill(3)
                                if self.TempBytes[19] != ord(CS_str[-3]) or self.TempBytes[20] != ord(CS_str[-2]) or self.TempBytes[21] != ord(CS_str[-1]):
                                    print("{} : 校验和不通过，整条数据都不对，全部删除, Check sum = ".format(self.deviceName) + CS_str + "原始数据 = " + str(self.TempBytes[0:22])) # 整条数据都不对，全部删除
                                    # del self.TempBytes[0:22] # 说明整条数据都不对
                                    # 查找下一个0x73的位置
                                    # try:
                                    #     del self.TempBytes[0]
                                    #     index = self.TempBytes(0x73)
                                    #     self.TempBytes = self.TempBytes[index:]
                                    # except ValueError:
                                    #     # 如果找不到0x73，删除全部数据
                                    #     self.TempBytes.clear()
                                    # continue
                                    del self.TempBytes[0]
                                else:
                                    self.processData()
                elif self.TempBytes[1] == 0x41: # 读取传感器地址
                    if len(self.TempBytes) < 6:
                        continue
                    else:
                        if self.TempBytes[4] != 0x0D or self.TempBytes[5] != 0x0A: # 数据尾不对
                            del self.TempBytes[0:6] # 说明整条数据都不对
                            continue
                        else:
                            devID = (self.TempBytes[2] - 0x30) * 10 + self.TempBytes[3] - 0x30
                            self.set("devID", devID)
                            # print("devID = {},调用！".format(devID))
                            self.idCheckCallback(self,devID,0x00)
                            del self.TempBytes[0:6] # 删除该条数据
                            continue
                else:
                    del self.TempBytes[0]
                    continue

    # 数据解析 data analysis
    def processData(self):
        # 　数据解析
        pressure = (-1 if self.TempBytes[4] == 0x31 else 1) * ((self.TempBytes[5] - 0x30) * 1000 + (self.TempBytes[6]-0x30) * 100 
                                                                + (self.TempBytes[7] - 0x30) * 10 + (self.TempBytes[8] - 0x30) \
                                                                + (self.TempBytes[9] - 0x30) / 10.0 + (self.TempBytes[10] - 0x30) / 100.0 \
                                                                + (self.TempBytes[11] - 0x30) / 1000.0 + (self.TempBytes[12] - 0x30) / 10000.0)
        temperature = (-1 if self.TempBytes[12] == 0x31 else 1) * ((self.TempBytes[14] - 0x30) * 10 + (self.TempBytes[15] - 0x30) 
                                                                + (self.TempBytes[16] - 0x30) / 10.0 + (self.TempBytes[17] - 0x30)/ 100.0 \
                                                                + (self.TempBytes[18] - 0x30) / 1000.0)
        self.set("pressure", round(pressure, 3))
        self.set("temperature", round(temperature, 2))
        self.callback_method(self)
        self.TempBytes.clear()

    # 发送串口数据 Sending serial port data
    def sendData(self, data):
        try:
            self.serialPort.write(data)
        except Exception as ex:
            self.idCheckCallback(self,0x00, 0x0a) # 设备未知错误
            print("{} : ERROR {}".format(self.deviceName,ex))

    def readSensor_ID(self):
        print("{} : 开始读取设备ID".format(self.deviceName))
        self.sendData(bytes([0x73, 0x41, 0x30, 0x30, 0x0D, 0x0A]))

    # 获得读取传感器数据的指令数据
    def readSensor_PackageData(self):
        ID_H_ASCII = ((int) (self.ADDR / 10) + 0x30) & 0xFF
        ID_L_ASCII = ((int) (self.ADDR % 10) + 0x30) & 0xFF
        tempBytes = bytes([0x73, 0x44, ID_H_ASCII, ID_L_ASCII, 0x0D, 0x0A])
        return tempBytes

    # 开始循环读取 Start loop reading
    def startLoopRead(self):
        # 循环读取控制
        self.loop = True
        # 开启读取线程 Enable read thread
        t = threading.Thread(target=self.loopRead, args=())
        t.start()

    # 循环读取线程 Loop reading data
    def loopRead(self):
        print("{} : 循环读取开始".format(self.deviceName))
        read_interval = 20000 # 10ms ->100hz 12.5ms -> 80 hz 20000->50hz
        while self.loop:
            loop_start_time = wiringpi.micros()
            self.sendData(self.readSensor_PackageData())
            elapsed_time = wiringpi.micros() - loop_start_time
            # 如果读取和写入数据的时间小于设定的时间间隔,则等待剩余时间
            if elapsed_time < read_interval:
                wiringpi.delayMicroseconds(read_interval - elapsed_time)
        print("{} : 循环读取结束".format(self.deviceName))

    # 关闭循环读取 Close loop reading
    def stopLoopRead(self):
        self.loop = False
